/****************************************************************************
 *
 * MODULE:             UDP Home sensor demo
 *
 *
 * REVISION:           $Revision: 10719 $
 *
 * DATED:              $Date: 2009-02-23 14:03:14 +0000 (Mon, 23 Feb 2009) $
 *
 *
 * AUTHOR:             DGAVE
 *
 * DESCRIPTION:
 * Regularly sends data from the sensors to a host using JIP UDP sockets.
 *
 *
 ****************************************************************************/
/***************************************************************************
 *
 *
 * This software is owned by Jennic and/or its supplier and is protected
 * under applicable copyright laws. All rights are reserved. We grant You,
 * and any third parties, a license to use this software solely and
 * exclusively on Jennic products. You, and any third parties must reproduce
 * the copyright and warranty notice and any other legend of ownership on each
 * copy or partial copy of the software.
 *
 * THIS SOFTWARE IS PROVIDED "AS IS". JENNIC MAKES NO WARRANTIES, WHETHER
 * EXPRESS, IMPLIED OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE,
 * ACCURACY OR LACK OF NEGLIGENCE. JENNIC SHALL NOT, IN ANY CIRCUMSTANCES,
 * BE LIABLE FOR ANY DAMAGES, INCLUDING, BUT NOT LIMITED TO, SPECIAL,
 * INCIDENTAL OR CONSEQUENTIAL DAMAGES FOR ANY REASON WHATSOEVER.
 *
 * Copyright Jennic Ltd 2008. All rights reserved
 *
 ***************************************************************************
 */
/****************************************************************************/
/***        Include files                                                 ***/
/****************************************************************************/

#include "JIP.h"
#include "Printf.h"
#include "SensorDemoConfig.h"
#include "Button.h"
#include "ALSdriver.h"
#include "HTSdriver.h"
#include "LedControl.h"
//#include "LcdDriver.h"
//#include "JennicLogo.h"

/****************************************************************************/
/***        Macro Definitions                                             ***/
/****************************************************************************/
#define NUMBER_OF_LEDS 2

/****************************************************************************/
/***        Type Definitions                                              ***/
/****************************************************************************/
/* Key (button) press values as used internally */
typedef enum
{
    E_KEY_0 = BUTTON_0_MASK, E_KEY_1 = BUTTON_1_MASK, E_KEYS_0_AND_1 = (BUTTON_0_MASK | BUTTON_1_MASK)
} teKeyValues;

/* State machine states */
typedef enum
{
    E_STATE_JOINING, E_STATE_READING_S1, E_STATE_READING_S2, E_STATE_TX_DATA, E_STATE_PAUSE
} teState;

/* State machine is as follows:

            Start
              |
              | Initialise.
              V
 +-----> E_STATE_JOINING
 |            |
 |            | Joined network
 |            V
 | +--> E_STATE_READING_S1
 | |          |
 | |          | Sensor read
 | |          V
 | |    E_STATE_READING_S2
 | |          |
 | |          | All sensors read
 | |          V
 | |    E_STATE_TX_DATA
 | |          |
 | |          | Transmission complete (success or fail)
 | |          V
 | |    E_STATE_PAUSE-----+
 | |          |           | Reset
 | |          | Timer     |
 | +----------+ fired     |
 |                        |
 +------------------------+
 */

/* All variables with scope throughout module are in one structure */
typedef struct
{
    /* Sensor data, stored between read and going out in frame */
    struct
    {
        uint16 u16TempResult;
        uint16 u16HumidityResult;
        uint16 u16LightResult;
        uint8 u8Switch;
    } sSensors;

    /* Settings used to flash LED 0 when joining the network */
    struct
    {
        uint8 u8Tick;   /* Incremented on timer callback */
        bool bLedState; /* Is the waiting for network LED currently off or on */
    } sJoiningTimer;

    /* Settings to allow control of flashing LEDs from remote host */
    struct
    {
        uint8 u8FlashRate; /* The rate to flash the LEDs (in 1/10 secs) */
        uint32 u32LastChangedTick; /* The last time the LED was updated */
        bool bCurrentState; /* Is the LED currently off or on */
    } sLedFlash[NUMBER_OF_LEDS];

    /* System (state, socket etc.) */
    struct
    {
        teState eState;
        int iSocket;
        bool bResetPending;     /* Has a reset been received but not acted on yet */
        uint32 u32LastPauseTick;
        uint32 iChildren;       /* The number of children off this device */
    } sSystem;
} tsDemoData;

/****************************************************************************/
/***        Local Function Prototypes                                     ***/
/****************************************************************************/
PRIVATE void vInitSystem(void);
PRIVATE void vIdleLoop(void);
PRIVATE void vStartReadSensors(void);
PRIVATE void vReadSensor2(void);
PRIVATE void vReadSensor3(void);
PRIVATE void vSendReadings(void);
PRIVATE void vPrintAddr(tsJIP_SockAddr *psSock);
PRIVATE void vTimerIntCallback(uint32 u32DeviceId, uint32 u32ItemBitmap);
//PRIVATE void vInitialiseLcd();
PUBLIC void Create_Socket(int PORT);
PUBLIC void Discovery(void);
PUBLIC void Send_Hello_Message(void);
PUBLIC void Send_Metadata(void);
PUBLIC void Create_Socket_Discovery(int PORT);
PRIVATE void PrintAddr2(tsJIP_SockAddr *psSock, uint8 *pString);

/****************************************************************************/
/***        Local Variables                                               ***/
/****************************************************************************/

/* File scope data.  Volatile because it is updated in the ISR and main loop */
volatile tsDemoData sDemoData;

/* Buffer passed to stack for packet management */
PRIVATE uint8 au8PacketBuffer[PACKET_BUFFER_SIZE] __attribute__ ((aligned (4)));


/* Number of the discovery socket, used to distinguish between data and discovery socket */
int iSocket_discovery = -1;

/* Variable for counting "w", used to trigger a reset if too many "w" */
volatile int count_w = 0;

/* Variable for counting ticks, used to trigger a reset */
//volatile int count_res = 0;

/* Variable for counting ticks, used to trigger HELLO messages */
volatile int count_hello = 0;

/* Store own IP address once and for all */
static tsJIP_SockAddr sOwnAddr;

/* Poor man's mutex */
static int mutex = 0;

/*
 * Buffer for the routing table.
 * 12 bytes per entry
 * Entries define the maximum number of children we can reach at once.
 *
 */
#define ROUTE_TABLE_CHILDREN 10
PRIVATE uint8 au8RoutingTable[ROUTE_TABLE_CHILDREN * 12 + 12] __attribute__ ((aligned (4)));

/****************************************************************************
 *
 * NAME: AppColdStart
 *
 * DESCRIPTION:
 * Entry point for application. Initialises system, starts scan then
 * processes interrupts.
 *
 * RETURNS:
 * void, never returns
 *
 ****************************************************************************/
PUBLIC void AppColdStart(void)
{
    //vInitialiseLcd();
    count_w = 0;
    count_hello = 0;
    mutex = 0;

    /* Set default values for the system data  */
    sDemoData.sSensors.u16TempResult = 0;
    sDemoData.sSensors.u16HumidityResult = 0;
    sDemoData.sSensors.u16LightResult = 0;
    sDemoData.sSensors.u8Switch = 0;
    sDemoData.sJoiningTimer.bLedState = FALSE;
    sDemoData.sJoiningTimer.u8Tick = 0;
    sDemoData.sSystem.eState = E_STATE_JOINING;
    sDemoData.sSystem.bResetPending = FALSE;
    sDemoData.sSystem.iChildren = 0;
    sDemoData.sSystem.u32LastPauseTick = 0;
    sDemoData.sSystem.iSocket = -1;

    /* Initialise the joining timer variables */
    sDemoData.sJoiningTimer.u8Tick = 0;
    sDemoData.sJoiningTimer.bLedState = FALSE;

     /* We default the LEDS to a slow 2 sec flash here. */
    int i;
    for (i = 0; i < NUMBER_OF_LEDS; i++)
    {
        sDemoData.sLedFlash[i].u8FlashRate = 20;
        sDemoData.sLedFlash[i].u32LastChangedTick = 0;
        sDemoData.sLedFlash[i].bCurrentState = FALSE;
    }


#ifdef HIGH_POWER_MODULE
        /* enable external PA for high power module */
        vAHI_HighPowerModuleEnable(TRUE, TRUE);
#endif
    vJIP_InitHardware();

    vUART_printInit();

    vPrintf("\nRouter Cold Start.\n");

    /* Initialise the timer that is used to control the LED flashing */
    vAHI_TimerEnable(E_AHI_TIMER_0,
                     16, /* Prescale value to divide the clock (2^16). */
                     FALSE,  /* Don't enable ints when the output goes high. */
                     TRUE,   /* Enable ints when timer period has completed. */
                     FALSE); /* Disable output of timer on associated PWM output pin */


    vAHI_TimerClockSelect(E_AHI_TIMER_0,
                          FALSE, /* FALSE to use internal 16MHz clock */
                          TRUE); /* TRUE to gate the output pin when the gate input is high. */

    vAHI_Timer0RegisterCallback(vTimerIntCallback);

    vAHI_TimerStartRepeat(E_AHI_TIMER_0,
                          FLASH_CLOCK_PERIOD >> 1, /* Clock periods after starting timer before output goes high. */
                          FLASH_CLOCK_PERIOD);     /* Clock periods after starting a timer before the */
                                                   /* output goes low. Effectively the time period of the whole cycle. */

    /* Sensor hardware initialisation */
    vInitSystem();

    /* Initialise the JIP stack */
    tsStackInitData sStackData;
    sStackData.eLinkLayer = E_LINKLAYER_JENNET; /* Run 6LowPAN on top of the Jennet network */
    sStackData.eDeviceType = E_JIP_ROUTER; /* We are a router */

    sStackData.u32RoutingTableEntries = ROUTE_TABLE_CHILDREN;
    sStackData.pvRoutingTableSpace = au8RoutingTable;

    sStackData.u16PanId = PAN_ID; /* PAN ID, channel and network ID must match the coordinator on the gateway */
    sStackData.u32Channel = CHANNEL_BITMAP;
    sStackData.u32NetworkId = NETWORK_APPLICATION_ID;

    /*
     * JIP 6LowPAN parameters
     */
    sStackData.u64AddressPrefix = ADDRESS_PREFIX; /* IPv6 address prefix - must match coordinator and gateway. */
    sStackData.pau8PacketBuffer = au8PacketBuffer; /* Buffer for JIP to store IPv6 packets. */
    sStackData.u32PacketBufferSize = PACKET_BUFFER_SIZE;
    sStackData.u8Sockets = 8; /* Maximum number of sockets that can be created. */
    sStackData.u16maxMTU = MAX_MTU; /* Maximum size of an IPv6 frame. */


    if (iJIP_InitStack(&sStackData) == -1)
    {
        /* Error! */
        vPrintf("Failed to start - iJIP_InitStack set error code %x.\n", u32JIP_GetErrNo());
    }
    else
    {
        vPrintf("InitStack returned OK.  Number of packet buffers %d.\n", sStackData.u16NumPacketBuffers);
    }


    (void) iJIP_GetOwnDeviceAddress(&sOwnAddr, TRUE);

    /* Everything after this is event driven */
    vIdleLoop();

    vPrintf("/!\\ SHOULD NOT OCCUR /!\\ fell out of vIdleLoop in AppColdStart. RESETTING\n");
    vAHI_SwReset();
}

/****************************************************************************
 *
 * NAME: AppWarmStart
 *
 * DESCRIPTION:
 * Warm start is called when waking from a sleep with the memory preserved.
 *
 * RETURNS:
 * Never returns.
 *
 ****************************************************************************/
PUBLIC void AppWarmStart(void)
{
    vPrintf("***Warm start called for a router***\n");

    /* Everything after this is event driven */
    vIdleLoop();

    vPrintf("/!\\ SHOULD NOT OCCUR /!\\ fell out of vIdleLoop in AppWarmStart. RESETTING\n");
    vAHI_SwReset();
}

/****************************************************************************/
/***        Callback functions                                            ***/
/****************************************************************************/

/****************************************************************************
 *
 * NAME: vJIP_StackEvent
 *
 * DESCRIPTION:
 * Processes any incoming stack events.  It is important here to ensure that
 * the application does
 * not attempt any communications until the device has joined the wireless
 * network.
 * Once a join indication has been received, we create a socket for future
 * communication.
 *
 * PARAMETERS: Name          RW Usage
 *             eEvent        R  Stack event
 *             pu8Data       R  Additional information associated with event
 *             u8DataLen     R  Length of additional information
 *
 ****************************************************************************/
PUBLIC void vJIP_StackEvent(teJIP_StackEvent eEvent, uint8 *pu8Data, uint8 u8DataLen)
{
    switch (eEvent)
    {
        case E_STACK_JOINED:
            /* It is possible to get spurious join events so check we haven't already joined. */
            if (sDemoData.sSystem.eState == E_STATE_JOINING)
            {
                vPrintf("\nJoinedNetwork\n");

                vPrintf("\n Create Socket on port 0xF0B1 (data)\n");
                Create_Socket(PC_UDP_PORT);

                vPrintf("\n End of Create Socket on port 0xF0B1 (data)\n");

                vPrintf("\n Create Socket on port 0xF0B2 (discovery)\n");
                Create_Socket_Discovery(DISCOVERY_PORT);
                vPrintf("\n End of Create Socket on port 0xF0B2 (discovery)\n");

                vStartReadSensors();
            }
            else
            {
                vPrintf("\nJOIN event received whilst already on network.\n");
            }
            break;

        case E_STACK_RESET:
            /*
             * The stack has reset itself.
             * Don't do anything immediately but wait until the sensors have been read
             * and the E_STATE_TX_DATA or PAUSE state has been entered in user context
             * as we don't want to disturb the sequence of interrupts that drive the state machine.
             */
            vPrintf("\nStack Reset\n");
            if (sDemoData.sSystem.eState != E_STATE_JOINING)
            {
                /* If the stack has been joined, reset on the next pause. */
                sDemoData.sSystem.bResetPending = TRUE;
            }
            break;
        case E_STACK_NODE_JOINED:
            vPrintf("\nA child has joined.  Device now has %d children\n", ++sDemoData.sSystem.iChildren);
            break;
        case E_STACK_NODE_LEFT:
            vPrintf("\nA child has left.  Device now has %d children\n", --sDemoData.sSystem.iChildren);
            break;

        default:
            break;
    }
}

/****************************************************************************
 *
 * NAME: vJIP_DataEvent
 *
 * DESCRIPTION:
 * Deals with any incoming data events.
 *
 * PARAMETERS: Name        RW  Usage
 *             iSocket     R   Socket on which packet received
 *             eEvent      R   Data event
 *             psAddr      R   Source address (for RX) or destination (for TX)
 *             u8AddrLen   R   Length of address
 *
 ****************************************************************************/
PUBLIC void vJIP_DataEvent(int iSocket, teJIP_DataEvent eEvent, tsJIP_SockAddr *psAddr, uint8 u8AddrLen)
{
    while (mutex == 1);
    mutex = 1;

    switch (eEvent)
    {
    case E_DATA_SEND_FAILED:
    case E_DATA_SENT:
        /*
         *  Our frame has been fully processed and its buffer has been freed
         *  (success or fail).
         */
        if (eEvent == E_DATA_SEND_FAILED)
        {
            vPrintf("Send failed. JIP error code %x\n", u32JIP_GetErrNo());
        }

        vPrintf("P");
        sDemoData.sSystem.eState = E_STATE_PAUSE;

        break;

    case E_DATA_RECEIVED:
        /*
         *  E_DATA_RECEIVED means some UDP data has arrived.
         *  We receive the data and check if is a message to change the flash rate of the LEDs
         */
        vPrintf("\nReceived data from \n");
        vPrintAddr(psAddr);

        tsJIP_SockAddr sSrcAddr;
        uint8 u8AddrLen = 0;
        uint8 pu8RxData[RECEIVE_PAYLOAD_SIZE];

        /*
         * iJIP_RecvFrom transfers the data from the JIP IPv6 buffer in which it has been received
         * to our data buffer pu8RxData.
         * The port of the iSocket parameter must match the destination port of the packet.
         * pu8RxData / RECEIVE_PAYLOAD_SIZE specify where to store the data.
         * They must be large enough to receive the entire UDP payload of the packet.
         * sSrcAddr / u8AddrLen allows us to see the address and source port of the sender of the packet.
         */
        //int iDataLen = 0;
        int iDataLen = iJIP_RecvFrom(iSocket, pu8RxData, RECEIVE_PAYLOAD_SIZE, 0 /* flags */, &sSrcAddr, &u8AddrLen);

        if (iDataLen == -1)
        {
            vPrintf("iJIP_RecvFrom failed with error code %x\n", u32JIP_GetErrNo());
            /*
             *  The most likely cause for a receive errors is that one of our parameters was incorrect.
             *  For example, if a packet arrives greater than the RECEIVE_PAYLOAD_SIZE, iJIP_RecvFrom
             *  will return -1.
             *  If iJIP_RecvFrom returns -1, JIP keeps the IPv6 buffer in which the packet has been received
             *  in order to allow us to try again.
             *  We call iJIP_RecvFrom with a NULL buffer pointer, which means
             *  throw the packet away and free up the IPv6 buffer.
             */
             iJIP_RecvFrom(iSocket, NULL, 0, 0, NULL, NULL);
        }
        else
        {

#if 0
            if (iSocket == sDemoData.sSystem.iSocket)
            {
                vPrintf("Receive :: data socket\n");

                // Data in hexa
                vPrintf("socket %d\t%d bytes(Hex): ", iSocket, iDataLen);
                int i=0;
                for (i=0; i < iDataLen; i++)
                {
                    vPrintf("%x ", pu8RxData[i]);
                }
                vPrintf("\n");
                // Data in char
                vPrintf("%d bytes(Char): ", iDataLen);
                int j=0;
                for (j=0; j < iDataLen; j++)
                {
                    vPrintf("%c ", pu8RxData[j]);
                }
                vPrintf("\r\n");


                /* If receive data for discovery protocol : EVENT */
                if (
#if USE_MULTICAST_MESSAGE
                       pu8RxData[71] == MESSAGE_WRAPPER
                    && pu8RxData[72] == DEVICE_PROTOCOL_VERSION_1
                    && pu8RxData[73] == DEVICE_PROTOCOL_VERSION_2
                    && pu8RxData[74] == DEVICE_EVENT_PREFIX
#else
                       pu8RxData[0] == DEVICE_PROTOCOL_VERSION_1
                    && pu8RxData[1] == DEVICE_PROTOCOL_VERSION_2
                    && pu8RxData[2] == DEVICE_EVENT_PREFIX
#endif
                   )
                {
                    // XXX XXX Assume we are polled to send measurement data.
                    vSendReadings();
                }

            }
            else
#endif
            if (iSocket == iSocket_discovery)
            {
                /* If receive data for discovery protocol : GETMETADATA */
                if (
#if USE_MULTICAST_MESSAGE
                       pu8RxData[71] == MESSAGE_WRAPPER
                    && pu8RxData[72] == DEVICE_PROTOCOL_VERSION_1
                    && pu8RxData[73] == DEVICE_PROTOCOL_VERSION_2
                    && pu8RxData[74] == DEVICE_GETMETADATA_PREFIX
#else
                       pu8RxData[0] == DEVICE_PROTOCOL_VERSION_1
                    && pu8RxData[1] == DEVICE_PROTOCOL_VERSION_2
                    && pu8RxData[2] == DEVICE_GETMETADATA_PREFIX
#endif
                   )
                {
                    vPrintf("Receive :: GETMETADATA\n");

                    /*vPrintf("%d bytes(Hex) - GETMETADATA: ", iDataLen);
                    int i=0;
                    for (i=0; i < iDataLen ; i++)
                    {
                        vPrintf("%x ", pu8RxData[i]);
                    }
                    vPrintf("\n");*/

                    /* Send METADATA */
                    //iJIP_Close (PC_UDP_PORT);
                    Send_Metadata();
                }
                else
                {
                    vPrintf("Receive :: other DISCOVERY\n");

                    // Data in hexa
                    /*vPrintf("%d bytes(Hex) - DISCOVERY: ", iDataLen);
                    int i=0;
                    for (i=0; i < iDataLen; i++)
                    {
                        vPrintf("%x ", pu8RxData[i]);
                    }
                    vPrintf("\n");

                    // Data in char
                    vPrintf("%d bytes(Char) - DISCOVERY: ", iDataLen);
                    int j=0;
                    for (j=0; j < iDataLen; j++)
                    {
                        vPrintf("%c ", pu8RxData[j]);
                    }
                    vPrintf("\r\n");*/
                }
            }
        }

        break;

    case E_IP_DATA_RECEIVED:
        /*
         * IP messages are packets that arrive at a coordinator with an off network address.
         * They are used to implement a border router, which would pass them to another device.
         * We should only receive them if we are a coordinator so we throw them away.
         */
        vPrintf("\nBorder Router Data Received\n");
        iJIP_IpRecvFrom(NULL, 0);
        break;

    case E_JIP_ICMP_MESSAGE:
        /*
         * ICMP messages are control messages such as a host unreachable.
         * We throw these messages away by passing NULL to iJIP_RecvFrom.
         */
        vPrintf("\nICMP Data Received\n");
        iJIP_RecvFrom(iSocket, NULL, 0, 0, NULL, NULL);

        break;

    default:
        break;
    }
    mutex = 0;
}

/****************************************************************************
 *
 * NAME: vJIP_PeripheralEvent
 *
 * DESCRIPTION:
 * Processes any incoming peripheral events. The end device is completely
 * event driven (the idle loop does nothing) and the events handled are:
 *   button pressed       set value to pass to coordinator
 *   wake timer fired     start sensor reading to kick state machine
 *   sensor DIO line set  continue sensor reading state machine
 *
 * PARAMETERS: Name          RW Usage
 *             u32Device     R  Device that caused peripheral event
 *             u32ItemBitmap R  Events within that peripheral
 *
 ****************************************************************************/
PUBLIC void vJIP_PeripheralEvent(uint32 u32Device, uint32 u32ItemBitmap)
{

     uint8 u8KeysDown;

    /*
     * Check for key press. We don't bother with a debouce
     * algorithm as in all cases repeated presses have same effect
     *  anyway. We also don't bother to check that the hardware
     * interrupt was caused by a key press
     */
    u8KeysDown = u8ButtonReadRfd();

    /* Keys control virtual switch value that is passed to the central controller  */
    switch (u8KeysDown)
    {
    case E_KEY_0:
        sDemoData.sSensors.u8Switch = 0;
        break;

    case E_KEY_1:
        sDemoData.sSensors.u8Switch = 1;
        break;
    }

    if (u32Device == E_AHI_DEVICE_SYSCTRL)
    {
        if (sDemoData.sSystem.eState != E_STATE_JOINING)
        {
            /* Check for DIO pin used by humidity/temperature sensor */
            if (u32ItemBitmap & HTS_DATA_DIO_BIT_MASK)
            {
                switch (sDemoData.sSystem.eState)
                {
                case E_STATE_READING_S1:
                    vReadSensor2();
                    break;

                case E_STATE_READING_S2:
                    vReadSensor3();
                    break;

                default:
                    break;
                }
            }
        }
    }
}

PUBLIC void vJIP_ConfigureNetwork(tsNetworkConfigData *psNetworkConfigData)
{
}

/****************************************************************************/
/****************************************************************************/
/***        Local Functions                                               ***/
/****************************************************************************/
/****************************************************************************/

/****************************************************************************/
/***        Initialisation Functions                                      ***/
/****************************************************************************/

/****************************************************************************
 *
 * NAME: vInitSystem
 *
 * DESCRIPTION:
 * Initialises stack and hardware. Also sets non-default values in the
 * 802.15.4 PIB and starts the first read of the light sensor. Subsequent
 * reads of this sensor occur automatically.
 *
 * RETURNS:
 * void
 *
 ****************************************************************************/
PRIVATE void vInitSystem(void)
{
    /* Initialise sensor hardware */

    /* Set LEDs on */
    vLedControl(0, TRUE);
    vLedControl(1, FALSE);
    vLedInitRfd();

    /* Init buttons */
    vButtonInitRfd();
    /* Enable interrupts for DIO buttons */
    vAHI_DioWakeEdge(0, BUTTON_ALL_MASK_RFD_PIN);
    vAHI_DioWakeEnable(BUTTON_ALL_MASK_RFD_PIN, 0);

    /* Set up interrupt for humidity/temp sensor (don't enable yet) */
    vAHI_DioWakeEdge(0, HTS_DATA_DIO_BIT_MASK);

    /* Set up peripheral hardware */
    vALSreset();
    vHTSreset();

    vPrintf("\nInitialising sensors.\n");

    /* Start ALS now: it automatically keeps re-sampling after this */
    vALSstartReadChannel(0);
}

/****************************************************************************
 *
 * NAME: vInitialiseLcd
 *
 * DESCRIPTION:
 * Initialise the LCD display and print the Jennic logo.
 * It doesn't matter if there is no LCD attached.
 *
 * Returns void
 *
 ****************************************************************************/
#if 0
void vInitialiseLcd()
{
    /* Start the LCD */
    vLcdResetDefault();

    /* The LCD uses a shadow buffer where the image is built and the buffer is then */
    /* updated to the display in a single slow operation. */
    /* Clear the shadow buffer */
    vLcdClear();

    /* Write the Jennic logo to the shadow buffer */
    vLcdWriteBitmap((tsBitmap *)&sJennicLogo, 0, 1);

    vLcdWriteText("UDP Home Sensor demo", 5, 0);

    /* Send the shadow buffer to the display. */
    vLcdRefreshAll();
}
#endif

/****************************************************************************
 *
 * NAME: vAddTextToLine
 *
 * DESCRIPTION:
 * Adds text to the global line buffer pLineBuf
 *
 * Parameters:
 * char *pString  The null terminated string to add
 *
 * Returns void
 *
 ****************************************************************************/
#define LINE_BUF_MAX 40
char pLineBuf[LINE_BUF_MAX + 1];
uint iLineBufIndex = 0;
void vAddTextToLine(char *pString)
{
    while(*pString != 0 && iLineBufIndex < LINE_BUF_MAX)
    {
        pLineBuf[iLineBufIndex++] = *pString++;
    }
}

/****************************************************************************
 *
 * NAME: vAddU8ToLine
 *
 * DESCRIPTION:
 * Adds the ASCII text for an unsigned 8 bit number to the line buffer
 *
 * Parameters:
 * uint8 u8Value  The number to add
 *
 * Returns void
 *
 ****************************************************************************/
#if 0
void vAddU8ToLine(uint8 u8Value)
{
    /* Check enough space for worst case of three digits */
    if (iLineBufIndex + 3 >= LINE_BUF_MAX)
        return;

    /* Add each digit */
    if (u8Value >= 100)
    {
        uint8 u8Digit = u8Value / 100;
        pLineBuf[iLineBufIndex++] = u8Digit + '0';
        u8Value -= u8Digit * 100;
    }
    if (u8Value >= 10)
    {
        uint8 u8Digit = u8Value / 10;
        pLineBuf[iLineBufIndex++] = u8Digit + '0';
        u8Value -= u8Digit * 10;
    }
    pLineBuf[iLineBufIndex++] = u8Value + '0';
}
#endif

/****************************************************************************
 *
 * NAME: vRefreshLcd
 *
 * DESCRIPTION:
 * Refresh the sensor readings on the bottom lines of the LCD display
 *
 * Returns void
 *
 ****************************************************************************/
#if 0
PRIVATE void vRefreshLcd()
{
    /* Clear the shadow buffer */
    vLcdClear();

    /* Build up the line in the line buffer */
    iLineBufIndex = 0;
    vAddTextToLine("Our Temp ");
    vAddU8ToLine(sDemoData.sSensors.u8TempResult);
    vAddTextToLine("  Light ");
    vAddU8ToLine(sDemoData.sSensors.u16LightResult);
    pLineBuf[iLineBufIndex] = 0; /* Add null terminator */
    vLcdWriteText(pLineBuf, 6, 0);

    iLineBufIndex = 0;
    vAddTextToLine("Humidity ");
    vAddU8ToLine(sDemoData.sSensors.u8HumidityResult);
    pLineBuf[iLineBufIndex] = 0; /* Add null terminator */
    vLcdWriteText(pLineBuf, 7, 0);

    /* Only re-write the shadow buffer to the area of the LCD which has changed */
    /* This is faster than a full refresh and keeps the logo intact despite the full buffer clear */
    vLcdRefreshArea(0,   /* Left Column */
                    6,   /* Top Row */
                    128, /* Width */
                    2);  /* Height */
}
#endif


/****************************************************************************/
/***        'Stack Interaction' Functions                                 ***/
/****************************************************************************/
// Found at http://www.jennic.com/support/solutions/00018
PUBLIC bool_t bUartIO_WaitEmptyTx (uint8 u8Uart)
{
    bool_t bReturn = FALSE;
    uint8   u8LineStatus;
    uint32 u32Safety;

    /* Wait (a limited time) for transmit queue to be empty */
    for (u32Safety = 0xFFF;
         u32Safety > 0 && bReturn == FALSE;
         u32Safety--)
    {
        /* Read uart line status */
        u8LineStatus = u8AHI_UartReadLineStatus(u8Uart);
        /* Is the fifo and buffer empty ? */
        if ((u8LineStatus & E_AHI_UART_LS_THRE) != 0 &&
            (u8LineStatus & E_AHI_UART_LS_TEMT) != 0)
        {
            /* Queue is now empty */
            bReturn = TRUE;
        }
    }

    return bReturn;
}

/****************************************************************************
 *
 * NAME: vPauseState
 *
 * DESCRIPTION:
 * Entered pause state, change the LED states if appropriate
 * then go to sleep.
 *
 * We will wake up with a warm start and call vStartReadSensors()
 * to start the read in E_STATE_READING_S1.
 *
 *
 ****************************************************************************/
PRIVATE void vPauseState(void)
{
    /*
     * Check for a reset
     */
    if (sDemoData.sSystem.bResetPending)
    {
        sDemoData.sSystem.bResetPending = FALSE;
        sDemoData.sSystem.eState = E_STATE_JOINING;
    }

    /* When we sleep, the UART will be turned off so wait for it to finish transmitting */
    bUartIO_WaitEmptyTx(E_AHI_UART_0);
    //while ((u8AHI_UartReadLineStatus(E_AHI_UART_0) & E_AHI_UART_LS_TEMT) == 0);

//    vRefreshLcd();

}
/****************************************************************************
 *
 * NAME: vIdleLoop
 *
 * DESCRIPTION:
 * Loops around in user mode waiting for a state change from an interrupt.
 * Flashes LEDs and prints out debug info.
 *
 * Note that the buttons and sensors are configured to generate interrupts
 * when they are active.
 *
 ****************************************************************************/
PRIVATE void vIdleLoop(void)
{
    uint32 i = 0;
    while (1)
    {
        if (sDemoData.sSystem.eState == E_STATE_PAUSE)
        {
            i = 0;
            vPauseState();
        }
        //if (sDemoData.sSystem.eState != E_STATE_JOINING)
        //{
            i++;
        //}
        if (i == 0x1000000) {
            vPrintf("/!\\ SAFETY RESET IN vIdleLoop /!\\\n");
            vAHI_SwReset();
        }
    }
}

/****************************************************************************
 *
 * NAME: vTimerIntCallback
 *
 * DESCRIPTION:
 * Callback that is triggered when Timer 0 expires.  Used to generate a
 * clock for LED flashing and debug output.
 *
 *
 ****************************************************************************/
PRIVATE void vTimerIntCallback(uint32 u32DeviceId, uint32 u32ItemBitmap)
{
    /* Check the interrupt is from the timer */
    if (u32DeviceId != E_AHI_DEVICE_TIMER0)
    {
        vPrintf("vTimerIntCallback with u32DeviceId %x != E_AHI_DEVICE_TIMER0 %x\n", u32DeviceId, E_AHI_DEVICE_TIMER0);
        return;
    }
    /* Check the mask for an int after the period rather than one edge of the timer clock. */
    if (!(u32ItemBitmap & E_AHI_TIMER_PERIOD_MASK))
    {
        vPrintf("vTimerIntCallback with u32ItemBitmap %x  E_AHI_TIMER_PERIOD_MASK %x\n", u32ItemBitmap, E_AHI_TIMER_PERIOD_MASK);
        return;
    }

    if (sDemoData.sSystem.eState == E_STATE_JOINING)
    {
        /* If we are waiting for the network, print a "w" and flash LED 0 once per second */
        if (sDemoData.sJoiningTimer.u8Tick++ > 10)
        {
            sDemoData.sJoiningTimer.u8Tick = 0;
            sDemoData.sJoiningTimer.bLedState
                    = !sDemoData.sJoiningTimer.bLedState;
            vLedControl(0, sDemoData.sJoiningTimer.bLedState);
            vPrintf("w");
            count_w++;
        }
        if (count_w == 20)
        {
            count_w = 0;
            vPrintf("count reset \n");
            vAHI_SwReset();
        }



    }
    else
    {
        /* If not joining, flash LEDs at programmed flash rate. */
        int i;
        for (i = 0; i < NUMBER_OF_LEDS; i++)
        {
            if (sDemoData.sLedFlash[i].u32LastChangedTick++
                    >= sDemoData.sLedFlash[i].u8FlashRate)
            {
                sDemoData.sLedFlash[i].u32LastChangedTick = 0;
                sDemoData.sLedFlash[i].bCurrentState
                        = !sDemoData.sLedFlash[i].bCurrentState;
                vLedControl(i, sDemoData.sLedFlash[i].bCurrentState);
            }
        }
    }

    /* RESET device every five minutes */
    //count_res++;
    //vPrintf(" \n %d \n",count_res);
    /*if (count_res == 2400)
    {
            count_res =0;
            vPrintf("\n RESET \n");
            vAHI_SwReset();

    }*/

    count_hello++;

    if (sDemoData.sSystem.eState == E_STATE_PAUSE)
    {
        /* Check for end of pause time */
        if (sDemoData.sSystem.u32LastPauseTick++ > PAUSE_TIMER_TICKS)
        {
            sDemoData.sSystem.u32LastPauseTick = 0;
            vStartReadSensors();
        }
    }

}


/****************************************************************************
 *
 * NAME: vStartReadSensors/vReadSensor2/vReadSensor3
 *
 * DESCRIPTION:
 * Gets the current readings from each sensor. Uses the DIO line for the HTS
 * sensor to signal when a value is ready to be read, as this allows the CPU
 * to doze during the reading calculation.
 *
 * RETURNS:
 * void
 *
 ****************************************************************************/
PRIVATE void vStartReadSensors(void)
{
    sDemoData.sSystem.eState = E_STATE_READING_S1;

    vPrintf("S1");

    /* Start to read temperature */
    vHTSstartReadTemp();

    /* Clear DIO event */
    u32AHI_DioWakeStatus();

    /* Enable interrupt on DIO to tell when read has completed */
    vAHI_DioWakeEnable(HTS_DATA_DIO_BIT_MASK, 0);
}

PRIVATE void vReadSensor2(void)
{
    sDemoData.sSystem.eState = E_STATE_READING_S2;

    vPrintf("2");

    /* Clear interrupt on DIO */
    vAHI_DioWakeEnable(0, HTS_DATA_DIO_BIT_MASK);

    /* Read temperature, 0-52 are acceptable */
    sDemoData.sSensors.u16TempResult = u16HTSreadTempResult();

    /* Start to read humidity */
    vHTSstartReadHumidity();

    /* Clear DIO event */
    u32AHI_DioWakeStatus();

    /* Enable interrupt on DIO to tell when read has completed */
    vAHI_DioWakeEnable(HTS_DATA_DIO_BIT_MASK, 0);
}

PRIVATE void vReadSensor3(void)
{
    vPrintf("3");

    /* Clear interrupt on DIO */
    vAHI_DioWakeEnable(0, HTS_DATA_DIO_BIT_MASK);

    /* Clear DIO event */
    u32AHI_DioWakeStatus();

    /* Read humidity sensor */
    sDemoData.sSensors.u16HumidityResult = u16HTSreadHumidityResult();

    /*
     * Read light level.  This sensor automatically starts
     * a new conversion afterwards so there is no need for a 'start read'
     * Light sensor is 0..4095 so divide by 16 to fit into 8 bits.
     */
    sDemoData.sSensors.u16LightResult = u16ALSreadChannelResult();

    if (!sDemoData.sSystem.bResetPending)
    {
        /* Send HELLO message xor sensor readings (don't interfere with the state machine by sending both) */
        if (count_hello >= 600) // 600 -> 1 minute
        {
            count_hello = 0;
            Send_Hello_Message();
        }
        else
        {
            /* Send Sensors Data */
            vSendReadings();
        }
    }
    else
    {
        /* Go to pause state without sending reading if stack down */
        vPrintf("Reset pending - not transmitting\n");
        sDemoData.sSystem.eState = E_STATE_PAUSE;
    }
}


/****************************************************************************
 *
 * NAME: vPrintAddr
 *
 * DESCRIPTION:
 * Print an IPv6 address and port to the debug function
 *
 * PARAMETERS:      Name    RW  Usage
 * tsJIP_SockAddr   psSock  R   The address to print
 *
 * RETURNS:
 * void
 *
 ****************************************************************************/

PRIVATE void Print8BitsToDecString(uint8 u8Data, uint8 *pString)
{
    *pString++ = (u8Data / 100) + '0';
    u8Data %= 100;
    *pString++ = (u8Data / 10) + '0';
    *pString++ = u8Data % 10 + '0';
}

PRIVATE void Print16BitsToDecString(uint16 u16Data, uint8 *pString)
{
    *pString++ = (u16Data / 10000) + '0';
    u16Data %= 10000;
    *pString++ = (u16Data / 1000) + '0';
    u16Data %= 1000;
    *pString++ = (u16Data / 100) + '0';
    u16Data %= 100;
    *pString++ = (u16Data / 10) + '0';
    *pString++ = u16Data % 10 + '0';
}

PRIVATE void Print16BitsToHexString(uint16 u16Data, uint8 *pString)
{
    int    i;
    uint8  u8Nybble;

    for (i = 12; i >= 0; i -= 4)
    {
        u8Nybble = (uint8)((u16Data >> i) & 0x0f);
        u8Nybble += '0';
        if (u8Nybble > '9')
            u8Nybble += 'A' - '9' - 1;

        *pString++ = u8Nybble;
    }
}


PRIVATE void vPrintAddr(tsJIP_SockAddr *psSock)
{
    vPrintf("addr =");
    int i = 0;
    for (i = 0; i < 8; i++)
    {
        vPrintf("%x", psSock->sin6_addr.s6_addr16[i]);
        if (i != 7)
            vPrintf(":");
    }

    vPrintf(", port = %d.\n", psSock->sin6_port);
}

PRIVATE void PrintAddr2(tsJIP_SockAddr *psSock, uint8 *pString)
{
    int i = 0;
    for (i = 0; i < 8; i++)
    {
        Print16BitsToHexString(psSock->sin6_addr.s6_addr16[i], pString);
        pString += 4;
        if (i != 7)
            *pString++ = ':';
    }
}


/****************************************************************************
 *
 * NAME: vSendReadings
 *
 * DESCRIPTION:
 * Creates a JIP iSendTo request for the sensor readings and passes it to the stack.
 *
 * RETURNS:
 * void
 *
 ****************************************************************************/

PRIVATE void vSendReadings()
{
    vPrintf("\nSend Data\n");

    uint8 *pu8Payload;
    uint8 *pu8Payload_orig;
    tsJIP_SockAddr sAddr;
    sAddr.sin6_family = E_JIP_PF_INET6;
    sAddr.sin6_addr.s6_addr32[0] = PC_ADDR_0;
    sAddr.sin6_addr.s6_addr32[1] = PC_ADDR_1;
    sAddr.sin6_addr.s6_addr32[2] = PC_ADDR_2;
    sAddr.sin6_addr.s6_addr32[3] = PC_ADDR_3;
    sAddr.sin6_port = PC_UDP_PORT;

   if (iJIP_GetDataBuffer(&pu8Payload_orig) == 0)
    {

        pu8Payload = pu8Payload_orig;

#if USE_MULTICAST_MESSAGE
        /* START MULTICAST MESSAGE */
        /* Prefix */
        *pu8Payload++ = MESSAGE_PROTOCOL_VERSION_1;
        *pu8Payload++ = MESSAGE_PROTOCOL_VERSION_2;
        /* From */
        PrintAddr2(&sOwnAddr, pu8Payload);
        pu8Payload += 8*4 + 7*1;
        /* Separator */
        *pu8Payload++ = MESSAGE_FIELD_SEPARATOR;
        /* To */
        PrintAddr2(&sAddr, pu8Payload);
        pu8Payload += 8*4 + 7*1;
        /* Separator */
        *pu8Payload++ = MESSAGE_FIELD_SEPARATOR;
        /* Port */
        Print16BitsToDecString(PC_UDP_PORT, pu8Payload);
        pu8Payload += 5;
        /* Separator */
        *pu8Payload++ = MESSAGE_FIELD_SEPARATOR;
        /* Payload separator */
        *pu8Payload++ = MESSAGE_WRAPPER;
#endif

        /* START DISCOVERY MESSAGE */
        /* Prefix */
        *pu8Payload++ = DEVICE_PROTOCOL_VERSION_1;
        *pu8Payload++ = DEVICE_PROTOCOL_VERSION_2;

        /* Lettre Send Data*/
        *pu8Payload++ = DEVICE_EVENT_PREFIX;
        /* id : MAC Adress */
        PrintAddr2(&sOwnAddr, pu8Payload);
        pu8Payload += 8*4 + 7*1;
        /* Separator */
        *pu8Payload++ = DEVICE_FIELD_SEPARATOR;
        /* Pairs Key/Value */
        /* Key 1 */
        *pu8Payload++ = 'T';
        *pu8Payload++ = 'e';
        *pu8Payload++ = 'm';
        *pu8Payload++ = 'p';
        *pu8Payload++ = '1';
        /* Separator Key/Value */
        *pu8Payload++ = DEVICE_EVENT_KV_SEPARATOR;
        /* Value 1 */
        Print8BitsToDecString(sDemoData.sSensors.u16TempResult, pu8Payload);
        pu8Payload += 3;

        /* Separator Pairs {Key/Value} */
        *pu8Payload++ = DEVICE_EVENT_FIELD_SEPARATOR;
        /* Key 2 */
        *pu8Payload++ = 'H';
        *pu8Payload++ = 'y';
        *pu8Payload++ = 'd';
        *pu8Payload++ = 'r';
        *pu8Payload++ = 'o';
        *pu8Payload++ = '1';
         /* Separator Key/Value */
        *pu8Payload++ = DEVICE_EVENT_KV_SEPARATOR;
        /* Value 2 */
        Print8BitsToDecString(sDemoData.sSensors.u16HumidityResult, pu8Payload);
        pu8Payload += 3;

        /* Separator Pairs {Key/Value} */
        *pu8Payload++ = DEVICE_EVENT_FIELD_SEPARATOR;
        /* Key 3 */
        *pu8Payload++ = 'L';
        *pu8Payload++ = 'i';
        *pu8Payload++ = 'g';
        *pu8Payload++ = 'h';
        *pu8Payload++ = 't';
        *pu8Payload++ = '1';
         /* Separator Key/Value */
        *pu8Payload++ = DEVICE_EVENT_KV_SEPARATOR;
        /* Value 3 */
        Print16BitsToDecString(sDemoData.sSensors.u16LightResult, pu8Payload);
        pu8Payload += 5;

        /* Separator */
        *pu8Payload++ = DEVICE_FIELD_SEPARATOR;
        /* END DISCOVERY MESSAGE */

#if USE_MULTICAST_MESSAGE
        /* Payload separator */
        *pu8Payload++ = MESSAGE_WRAPPER;
        /* END MULTICAST MESSAGE */
#endif

        /* Pad the remainder of the buffer */
        /*for (i = 10; i < TRANSMIT_PAYLOAD_SIZE; i++)
        {
            pu8Payload[i] = i & 0xff;
        }*/

         sDemoData.sSystem.eState = E_STATE_TX_DATA;

        /* Transmit the data in the buffer.*/
        if (iJIP_SendTo(sDemoData.sSystem.iSocket, pu8Payload_orig, pu8Payload - pu8Payload_orig, 0, &sAddr, sizeof(tsJIP_SockAddr)) == -1)
        {
            vPrintf("\niJIP_SendTo failed with error code %x.\n", u32JIP_GetErrNo());
        }
        else
        {
            vPrintf("Tx - ");
            int i;
            for (i=0; i < pu8Payload - pu8Payload_orig; i++)
            {
                vPrintf("%c", pu8Payload_orig[i]);
            }
            vPrintf("\n");

        }
    }
    else
    {
        vPrintf("\nSend FAILED - out of buffers\n");
    }
}


/****************************************************************************
 *
 * NAME: Discovery
 *
 * DESCRIPTION: Exchange with discovery app
 *
 * PARAMETERS: void
 *
 * RETURNS: void
 *
 ****************************************************************************/
PUBLIC void Discovery()
{
    vPrintf("\nDiscovery\n");

    //Send Hello Message for Discovery
    Send_Hello_Message();
}

/****************************************************************************/
/***        FONCTIONS FOR DISCOVERY                                        **/
/****************************************************************************/

/****************************************************************************
 *
 * NAME: Create_Socket
 *
 * DESCRIPTION:
 * Create Socket for transmissions
 *
 * PARAMETERS: void
 *
 * RETURNS:
 * void
 *
 ****************************************************************************/
PUBLIC void Create_Socket(int PORT)
{
    if (sDemoData.sSystem.iSocket == -1)
    {
        vPrintf("Creating socket\n");
        sDemoData.sSystem.iSocket = iJIP_Socket(E_JIP_PF_INET6, E_JIP_SOCK_DGRAM, E_JIP_PROTOCOL_ONLY_ONE);
        if (sDemoData.sSystem.iSocket == -1)
        {
            /* Error! */
            vPrintf("Unable to create socket.  Error code %x.\n", u32JIP_GetErrNo());
            return;
        }
        vPrintf("Socket %d created OK.\n", sDemoData.sSystem.iSocket);
        /* Bind socket: this is to enable source port to be specified
         * The TRUE parameter means generate a global address (2001:1000:)
         * rather than a link local (fe80::)
         * This is important when sending through a gateway, so the remote PC
         * knows the full address to send commands to.
         */
        sOwnAddr.sin6_port = PORT;

        if (iJIP_Bind(sDemoData.sSystem.iSocket, &sOwnAddr, sizeof(tsJIP_SockAddr)) == -1)
        {
            /* Error! */
            vPrintf("Unable to bind socket.  Error code %x.\n", u32JIP_GetErrNo());
            return;
        }

        /*
         * The defrag timer is used when receiving fragments of a single IPv6 packet.
         * It controls how long the buffer is held for until we give up assembling the packet.
         * It defaults to 60 seconds.  1 second is a better choice when we have a limited
         * number of buffers.  Otherwise, if a part of a fragment gets lost, the buffer is held
         * for a minute.  If this happens several times in one minute, we can run out of buffers.
         */
        vJIP_SetPacketDefragTimeout(5);
    }
}

PUBLIC void Create_Socket_Discovery(int PORT)
{
    if (iSocket_discovery == -1)
    {
        /*
         * Create a new socket.
         * The flags must be as below, JIP currently only supports IPv6 UDP sockets.
         */
         vPrintf("Creating socket\n");
        iSocket_discovery = iJIP_Socket(E_JIP_PF_INET6, E_JIP_SOCK_DGRAM, E_JIP_PROTOCOL_ONLY_ONE);

        if (iSocket_discovery == -1)
        {
            /* Error! */
            vPrintf("Unable to create socket.  Error code %x.\n", u32JIP_GetErrNo());
            return;
        }

        vPrintf("Socket %d created OK.\n", iSocket_discovery);
        /* Bind socket: this is to enable source port to be specified
         * The TRUE parameter means generate a global address (2001:1000:)
         * rather than a link local (fe80::)
         * This is important when sending through a gateway, so the remote PC
         * knows the full address to send commands to.
         */
        sOwnAddr.sin6_port = PORT;

        if (iJIP_Bind(iSocket_discovery, &sOwnAddr, sizeof(tsJIP_SockAddr)) == -1)
        {
            /* Error! */
            vPrintf("Unable to bind socket.  Error code %x.\n", u32JIP_GetErrNo());
            return;
        }
    }
}


/****************************************************************************
 *
 * NAME: Send_Hello_Message
 *
 * DESCRIPTION:
 * Send Hello Message
 *
 * PARAMETERS: void
 *
 * RETURNS:
 * void
 *
 ****************************************************************************/
 PUBLIC void Send_Hello_Message()
 {

    vPrintf("Sending Hello Message \n");

    uint8 *pu8Payload;
    uint8 *pu8Payload_orig;
    tsJIP_SockAddr sAddr;
    sAddr.sin6_family = E_JIP_PF_INET6;
    sAddr.sin6_addr.s6_addr32[0] = PC_ADDR_0;
    sAddr.sin6_addr.s6_addr32[1] = PC_ADDR_1;
    sAddr.sin6_addr.s6_addr32[2] = PC_ADDR_2;
    sAddr.sin6_addr.s6_addr32[3] = PC_ADDR_3;
    sAddr.sin6_port = DISCOVERY_PORT;

    if (iJIP_GetDataBuffer(&pu8Payload_orig) == 0)
    {
        pu8Payload = pu8Payload_orig;

#if USE_MULTICAST_MESSAGE
        /* START MULTICAST MESSAGE */
        /* Prefix */
        *pu8Payload++ = MESSAGE_PROTOCOL_VERSION_1;
        *pu8Payload++ = MESSAGE_PROTOCOL_VERSION_2;
        /* From */
        PrintAddr2(&sOwnAddr, pu8Payload);
        pu8Payload += 8*4 + 7*1;
        /* Separator */
        *pu8Payload++ = MESSAGE_FIELD_SEPARATOR;
        /* To */
        PrintAddr2(&sAddr, pu8Payload);
        pu8Payload += 8*4 + 7*1;
        /* Separator */
        *pu8Payload++ = MESSAGE_FIELD_SEPARATOR;
        /* Port */
        Print16BitsToDecString(DISCOVERY_PORT, pu8Payload);
        pu8Payload += 5;
        /* Separator */
        *pu8Payload++ = MESSAGE_FIELD_SEPARATOR;
        /* Payload separator */
        *pu8Payload++ = MESSAGE_WRAPPER;
#endif

        /* START DISCOVERY MESSAGE */
        /* Prefix */
        *pu8Payload++ = DEVICE_PROTOCOL_VERSION_1;
        *pu8Payload++ = DEVICE_PROTOCOL_VERSION_2;
        /* Lettre HELLO */
        *pu8Payload++ = DEVICE_HELLO_PREFIX;
        /* id : MAC Adress */
        PrintAddr2(&sOwnAddr, pu8Payload);
        pu8Payload += 8*4 + 7*1;
        /* Separator */
        *pu8Payload++ = DEVICE_FIELD_SEPARATOR;
        /* ip : ipv6 address */
        PrintAddr2(&sOwnAddr, pu8Payload);
        pu8Payload += 8*4 + 7*1;
        /* Separator */
        *pu8Payload++ = DEVICE_FIELD_SEPARATOR;
        Print16BitsToDecString(DISCOVERY_PORT, pu8Payload);
        pu8Payload += 5;
        /* Separator */
        *pu8Payload++ = DEVICE_FIELD_SEPARATOR;
        /* END DISCOVERY MESSAGE */

#if USE_MULTICAST_MESSAGE
        /* Payload separator */
        *pu8Payload++ = MESSAGE_WRAPPER;
        /* END MULTICAST MESSAGE */
#endif

        /* Pad the remainder of the buffer */
        /*for ( ; pu8Payload < pu8Payload_orig + TRANSMIT_PAYLOAD_SIZE; )
        {
            *pu8Payload = (pu8Payload - pu8Payload_orig) & 0xff;
            pu8Payload++;
        }*/
        sDemoData.sSystem.eState = E_STATE_TX_DATA;

        /* Transmit the data in the buffer.*/
        if (iJIP_SendTo(iSocket_discovery, pu8Payload_orig, pu8Payload - pu8Payload_orig, 0, &sAddr, sizeof(tsJIP_SockAddr)) == -1)
        {
            vPrintf("\niJIP_SendTo failed with error code %x.\n", u32JIP_GetErrNo());
        }
        else
        {


            vPrintf("HELLO Message: ");
            /*int i=0;
            for (i=0; i < TRANSMIT_PAYLOAD_SIZE; i++)
            {
                vPrintf("%x ", pu8Payload_orig[i]);
            }
            vPrintf("\n");
            for (i=0; i < TRANSMIT_PAYLOAD_SIZE; i++)
            {
                vPrintf("%c", pu8Payload_orig[i]);
            }
            vPrintf("\n");*/

        }
    }
    else
    {
        vPrintf("\nSend FAILED - out of buffers\n");
    }
 }



  /****************************************************************************
 *
 * NAME: Send_Metadata
 *
 * DESCRIPTION:
 * Send Metadata
 *
 * PARAMETERS: void
 *
 * RETURNS: void
 *
 ****************************************************************************/
PUBLIC void Send_Metadata()
{
    vPrintf("\n Send Metadata for discovery \n");

    uint8 *pu8Payload;
    uint8 *pu8Payload_orig;
    tsJIP_SockAddr sAddr;
    sAddr.sin6_family = E_JIP_PF_INET6;
    sAddr.sin6_addr.s6_addr32[0] = PC_ADDR_0;
    sAddr.sin6_addr.s6_addr32[1] = PC_ADDR_1;
    sAddr.sin6_addr.s6_addr32[2] = PC_ADDR_2;
    sAddr.sin6_addr.s6_addr32[3] = PC_ADDR_3;
    sAddr.sin6_port = DISCOVERY_PORT;

   if (iJIP_GetDataBuffer(&pu8Payload_orig) == 0)
    {
        pu8Payload = pu8Payload_orig;

#if USE_MULTICAST_MESSAGE
        /* START MULTICAST MESSAGE */
        /* Prefix */
        *pu8Payload++ = MESSAGE_PROTOCOL_VERSION_1;
        *pu8Payload++ = MESSAGE_PROTOCOL_VERSION_2;
        /* From */
        PrintAddr2(&sOwnAddr, pu8Payload);
        pu8Payload += 8*4 + 7*1;
        /* Separator */
        *pu8Payload++ = MESSAGE_FIELD_SEPARATOR;
        /* To */
        PrintAddr2(&sAddr, pu8Payload);
        pu8Payload += 8*4 + 7*1;
        /* Separator */
        *pu8Payload++ = MESSAGE_FIELD_SEPARATOR;
        /* Port */
        Print16BitsToDecString(DISCOVERY_PORT, pu8Payload);
        pu8Payload += 5;
        /* Separator */
        *pu8Payload++ = MESSAGE_FIELD_SEPARATOR;
        /* Payload separator */
        *pu8Payload++ = MESSAGE_WRAPPER;
#endif

        /* START DISCOVERY MESSAGE */
        /* Prefix */
        *pu8Payload++ = DEVICE_PROTOCOL_VERSION_1;
        *pu8Payload++ = DEVICE_PROTOCOL_VERSION_2;

        /* Lettre METADATA*/
        *pu8Payload++ = DEVICE_METADATA_PREFIX;
        /* id : MAC Adress */
        PrintAddr2(&sOwnAddr, pu8Payload);
        pu8Payload += 8*4 + 7*1;
        /* Separator */
        *pu8Payload++ = DEVICE_FIELD_SEPARATOR;
        /* Pairs Key/Value */
        /* Key 1 */
        *pu8Payload++ = 'T';
        *pu8Payload++ = 'e';
        *pu8Payload++ = 'm';
        *pu8Payload++ = 'p';
        *pu8Payload++ = '1';
        /* Separator Key/Value */
        *pu8Payload++ = DEVICE_METADATA_KV_SEPARATOR;
        /* Value 1 */
        *pu8Payload++ = 'S';
        *pu8Payload++ = 'I';
        *pu8Payload++ = '-';
        *pu8Payload++ = 'C';
        /* Separator Pairs {Key/Value} */
        *pu8Payload++ = DEVICE_METADATA_FIELD_SEPARATOR;
        /* Key 2 */
        *pu8Payload++ = 'H';
        *pu8Payload++ = 'y';
        *pu8Payload++ = 'd';
        *pu8Payload++ = 'r';
        *pu8Payload++ = 'o';
        *pu8Payload++ = '1';
         /* Separator Key/Value */
        *pu8Payload++ = DEVICE_METADATA_KV_SEPARATOR;
        /* Value 2 */
        *pu8Payload++ = 'S';
        *pu8Payload++ = 'I';
        *pu8Payload++ = '-';
        *pu8Payload++ = '%';
        /* Separator Pairs {Key/Value} */
        *pu8Payload++ = DEVICE_METADATA_FIELD_SEPARATOR;
        /* Key 3 */
        *pu8Payload++ = 'L';
        *pu8Payload++ = 'i';
        *pu8Payload++ = 'g';
        *pu8Payload++ = 'h';
        *pu8Payload++ = 't';
        *pu8Payload++ = '1';
         /* Separator Key/Value */
        *pu8Payload++ = DEVICE_METADATA_KV_SEPARATOR;
        /* Value 3 */
        *pu8Payload++ = 'S';
        *pu8Payload++ = 'I';
        *pu8Payload++ = '-';
        *pu8Payload++ = 'l';
        *pu8Payload++ = 'x';
        /* Separator */
        *pu8Payload++ = DEVICE_FIELD_SEPARATOR;
        /* END DISCOVERY MESSAGE */

#if USE_MULTICAST_MESSAGE
        /* Payload separator */
        *pu8Payload++ = MESSAGE_WRAPPER;
        /* END MULTICAST MESSAGE */
#endif

        /* Pad the remainder of the buffer */
        /*for ( ; pu8Payload < pu8Payload_orig + TRANSMIT_PAYLOAD_SIZE; )
        {
            *pu8Payload = (pu8Payload - pu8Payload_orig) & 0xff;
            pu8Payload++;
        }*/
        sDemoData.sSystem.eState = E_STATE_TX_DATA;

        /* Transmit the data in the buffer.*/
        if (iJIP_SendTo(iSocket_discovery, pu8Payload_orig, /*TRANSMIT_PAYLOAD_SIZE*/pu8Payload - pu8Payload_orig, 0, &sAddr, sizeof(tsJIP_SockAddr)) == -1)
        {
            vPrintf("\niJIP_SendTo failed with error code %x.\n", u32JIP_GetErrNo());
        }
        else
        {


            vPrintf("METADATA Message: ");
            /*int i=0;
            for (i=0; i < TRANSMIT_PAYLOAD_SIZE; i++)
            {
                vPrintf("%x ", pu8Payload_orig[i]);
            }
            vPrintf("\n");
            for (i=0; i < TRANSMIT_PAYLOAD_SIZE; i++)
            {
                vPrintf("%c", pu8Payload_orig[i]);
            }
            vPrintf("\n");*/

        }
    }
    else
    {
        vPrintf("\nSend FAILED - out of buffers\n");
    }
}

/****************************************************************************/
/***        END OF FILE                                                   ***/
/****************************************************************************/
